#include "Stdafx.h"

void      load_cp_cc_data (FILE *fp, struct camera_parameters *cp,struct calibration_constants* cc)
{
	double    sa,
		ca,
		sb,
		cb,
		sg,
		cg;

	fscanf (fp, "%lf", &(cp->Ncx));
	fscanf (fp, "%lf", &(cp->Nfx));
	fscanf (fp, "%lf", &(cp->dx));
	fscanf (fp, "%lf", &(cp->dy));
	fscanf (fp, "%lf", &(cp->dpx));
	fscanf (fp, "%lf", &(cp->dpy));
	fscanf (fp, "%lf", &(cp->Cx));
	fscanf (fp, "%lf", &(cp->Cy));
	fscanf (fp, "%lf", &(cp->sx));

	fscanf (fp, "%lf", &(cc->f));
	fscanf (fp, "%lf", &(cc->kappa1));
	fscanf (fp, "%lf", &(cc->Tx));
	fscanf (fp, "%lf", &(cc->Ty));
	fscanf (fp, "%lf", &(cc->Tz));
	fscanf (fp, "%lf", &(cc->Rx));
	fscanf (fp, "%lf", &(cc->Ry));
	fscanf (fp, "%lf", &(cc->Rz));

	SINCOS (cc->Rx, sa, ca);
	SINCOS (cc->Ry, sb, cb);
	SINCOS (cc->Rz, sg, cg);

	cc->r1 = cb * cg;
	cc->r2 = cg * sa * sb - ca * sg;
	cc->r3 = sa * sg + ca * cg * sb;
	cc->r4 = cb * sg;
	cc->r5 = sa * sb * sg + ca * cg;
	cc->r6 = ca * sb * sg - cg * sa;
	cc->r7 = -sb;
	cc->r8 = cb * sa;
	cc->r9 = ca * cb;

	fscanf (fp, "%lf", &(cc->p1));
	fscanf (fp, "%lf", &(cc->p2));
}
void      distorted_to_undistorted_sensor_coord (double Xd,double Yd,double& Xu,double& Yu,struct camera_parameters cp,struct calibration_constants cc)
{
	double    distortion_factor;

	/* convert from distorted to undistorted sensor plane coordinates */
	distortion_factor = 1 + cc.kappa1 * (SQR (Xd) + SQR (Yd));
	Xu = Xd * distortion_factor;
	Yu = Yd * distortion_factor;
}
void      image_coord_to_world_coord (double Xfd,double Yfd,double zw,double& xw,double& yw,struct camera_parameters cp,struct calibration_constants cc)
{
	double    Xd,
		Yd,
		Xu,
		Yu,
		common_denominator;

	/* convert from image to distorted sensor coordinates */
	Xd = cp.dpx * (Xfd - cp.Cx) / cp.sx;
	Yd = cp.dpy * (Yfd - cp.Cy);

	/* convert from distorted sensor to undistorted sensor plane coordinates */
	distorted_to_undistorted_sensor_coord (Xd, Yd, Xu, Yu,cp,cc);

	/* calculate the corresponding xw and yw world coordinates   */
	/* (these equations were derived by simply inverting     */
	/* the perspective projection equations using Macsyma)   */
	common_denominator = ((cc.r1 * cc.r8 - cc.r2 * cc.r7) * Yu +
		(cc.r5 * cc.r7 - cc.r4 * cc.r8) * Xu -
		cc.f * cc.r1 * cc.r5 + cc.f * cc.r2 * cc.r4);

	//printf("com denom = %f",common_denominator);

	xw = (((cc.r2 * cc.r9 - cc.r3 * cc.r8) * Yu +
		(cc.r6 * cc.r8 - cc.r5 * cc.r9) * Xu -
		cc.f * cc.r2 * cc.r6 + cc.f * cc.r3 * cc.r5) * zw +
		(cc.r2 * cc.Tz - cc.r8 * cc.Tx) * Yu +
		(cc.r8 * cc.Ty - cc.r5 * cc.Tz) * Xu -
		cc.f * cc.r2 * cc.Ty + cc.f * cc.r5 * cc.Tx) / common_denominator;

	yw = -(((cc.r1 * cc.r9 - cc.r3 * cc.r7) * Yu +
		(cc.r6 * cc.r7 - cc.r4 * cc.r9) * Xu -
		cc.f * cc.r1 * cc.r6 + cc.f * cc.r3 * cc.r4) * zw +
		(cc.r1 * cc.Tz - cc.r7 * cc.Tx) * Yu +
		(cc.r7 * cc.Ty - cc.r4 * cc.Tz) * Xu -
		cc.f * cc.r1 * cc.Ty + cc.f * cc.r4 * cc.Tx) / common_denominator;
}